public class ControllerResponseTester {
	/**
	 * 	@author Paweł Jankowski
	 */

    private int gyrobegin = -30;
    private int gyroend = 30;
    private int anglebegin = -20;
    private int angleend = 20;

    private FuzzyController fc;
    private NXTFuzzyLogic nfl;
    
    public ControllerResponseTester(int gyrorange, int anglerange) {
        fc = new FuzzyController();

        gyrobegin = (int) -gyrorange/2;
        gyroend = -gyrobegin;

        anglebegin = (int) -anglerange/2;
        angleend = -anglebegin;
    }

    public ControllerResponseTester() {
        fc = new FuzzyController();
    }

    public void saveToFileFCL(){
        FileSaver p = new FileSaver();
        StringBuilder sb = new StringBuilder();

        for(int i = gyrobegin; i <= gyroend; i+=1){
            for(int j = anglebegin; j <= angleend; j+=1){
                fc.setInput(i, j);
                sb.append(i+";"+j+";"+fc.getOutput()+";\n");
            }
        }

        try{
             p.save("fcl.txt", sb.toString());
        }
        catch(Exception e){
            e.printStackTrace();
        }
    }

    public void saveToFileNXTFuzzyLogic(){
        nfl = new NXTFuzzyLogic();

        nfl.defineInputLinguisticVariable("vel", gyrobegin, gyroend, 0);
        nfl.defineInputLinguisticVariable("angle", anglebegin, angleend, 0);

        nfl.defineOutputLinguisticVariable("speed", -150, 150, 0, NXTFuzzyLogic.COG);

        nfl.defineTermRectangle("backSmall", "vel", -20, -10, -10, -1);
        nfl.defineTermRectangle("zero", "vel", -2, 0, 0, 2);
        nfl.defineTermRectangle("fowSmall", "vel", 1, 10, 10, 20);

        nfl.defineTermRectangle("negSmall", "angle", -14, -7, -7, 0);
        nfl.defineTermRectangle("zeroAng", "angle", -1, 0, 0, 1);
        nfl.defineTermRectangle("posSmall", "angle", 0, 7, 7, 14);

        nfl.defineTermRectangle("backFast","speed",-100, -50, -50, 0);
        nfl.defineTermRectangle("backSlow", "speed",-50, -25, -25, 0);
        nfl.defineTermRectangle("stop", "speed", -1, 0, 0, 1);
        nfl.defineTermRectangle("fowSlow", "speed", 0, 25, 25, 50);
        nfl.defineTermRectangle("fowFast", "speed", 0, 50, 50, 100);

        String[] cond0 = {"negSmall", "backSmall"};
        nfl.defineRule(cond0, "backFast", NXTFuzzyLogic.MIN);
        String[] cond1 = {"negSmall", "zero"};
        nfl.defineRule(cond1, "backFast", NXTFuzzyLogic.MIN);
        String[] cond2 = {"negSmall", "fowSmall"};
        nfl.defineRule(cond2, "backSlow", NXTFuzzyLogic.MIN);
        String[] cond3 = {"zeroAng", "backSmall"};
        nfl.defineRule(cond3, "backSlow", NXTFuzzyLogic.MIN);
        String[] cond4 = {"zeroAng", "zero"};
        nfl.defineRule(cond4, "stop", NXTFuzzyLogic.MIN);
        String[] cond5 = {"zeroAng", "fowSmall"};
        nfl.defineRule(cond5, "fowSlow", NXTFuzzyLogic.MIN);
        String[] cond6 = {"posSmall", "backSmall"};
        nfl.defineRule(cond6, "fowSlow", NXTFuzzyLogic.MIN);
        String[] cond7 = {"posSmall", "zero"};
        nfl.defineRule(cond7, "fowFast", NXTFuzzyLogic.MIN);
        String[] cond8 = {"posSmall", "fowSmall"};
        nfl.defineRule(cond8, "fowFast", NXTFuzzyLogic.MIN);


//        String[] r1= {"backSmall"};
//        nfl.defineRule(r1, "backSlow", NXTFuzzyLogic.MIN);
//        String[] r2 = {"zero"};
//        nfl.defineRule(r2, "stop", NXTFuzzyLogic.MIN);
//        String[] r3 = {"fowSmall"};
//        nfl.defineRule(r3, "fowSlow", NXTFuzzyLogic.MIN);
        nfl.init();

        FileSaver p = new FileSaver();
        StringBuilder sb = new StringBuilder();

        for(int i = gyrobegin; i <= gyroend; i+=1){
            for(int j = anglebegin; j <= angleend; j+=1){
                nfl.setInputValue("vel", i);
                nfl.setInputValue("angle", j);
                nfl.evaluate();
                float z = nfl.getOutputValue("speed");
                sb.append(i+";"+j+";"+z+";\n");
            }
        }

        try{
             p.save("nfl.txt", sb.toString());
        }
        catch(Exception e){
            e.printStackTrace();
        }
    }
    
}
